/**  
 * Copyright (c) 2010 University of Pennsylvania.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 */
 
//#define TINYOS2               // UPDATE THIS IN BOTH *M.nc and *.nc!
#define SENSORBOARD_ITS400      // UPDATE THIS IN BOTH *M.nc and *.nc!
//#define SENSORBOARD_MTS400CB  // UPDATE THIS IN BOTH *M.nc and *.nc!
#define USE_BLUSH               // UPDATE THIS IN BOTH *M.nc and *.nc!

#define USE_VIRTUALADDR         // if defined, use virtual addressing, requires snooping

#ifndef TOSSIM
#define SAVE_DEBUG              // if defined, will save debug messages in memory, send to uart later
#define USE_MEMFILES            // if defined, will generate in memory logs on stat request
#endif

#ifdef SENSORBOARD_ITS400
#define EVENLENGTH_MSG          // maximum message length must be even for imote2's
#endif

#define DISABLE_HARDWAREACK
//#define MINIMUM_MEMORY

#ifdef SENSORBOARD_ITS400
#define PACKING_MACRO           // imote2 doesnt behave well with packing
#else
#define PACKING_MACRO __attribute((packed))   
#endif

#ifdef TINYOS2
#include <message.h>
#else
#ifdef USE_BLUSH
includes trace;
#endif
#ifdef SENSORBOARD_ITS400
#include "pmic.h" 
#endif
#endif

module streamintM {
#ifndef TINYOS2
  provides {
    interface StdControl;
#ifdef USE_BLUSH
    interface BluSH_AppI as BlushCommand; 
    interface BluSH_AppI as BlushAttr; 
    interface BluSH_AppI as BlushDebug;
#endif     
  }
#endif

  uses {
    interface Leds;
#ifndef TOSSIM    
    interface Reset;
#endif	
    
#ifdef TINYOS2	
    interface Boot;
    interface Receive as ReceiveT;
    interface Receive as ReceiveS;
    interface AMSend;
    interface Timer<TMilli> as Timer1;
    interface Timer<TMilli> as Timer2;
    interface Packet;
    interface AMPacket;
    interface SplitControl as RadioControl;
    interface SplitControl as UARTControl;
    interface PacketAcknowledgements as PacketAck;
    interface Receive as ReceiveUART;
    interface AMSend as SendUART;
#ifndef TOSSIM
    interface CC2420Packet;
#endif    
#else
    interface Timer as Timer1;  // main timer  
    interface Timer as Timer2;  // randomizer
    interface ReceiveMsg;
    interface BareSendMsg;
    interface StdControl as CommControl;
    interface CC2420Control;
    
#ifdef USE_BLUSH
    interface Timer as Timer3;  // debug timer
#else    
    interface StdControl as UARTControl;
    interface BareSendMsg as UARTSend;
    interface ReceiveMsg as UARTReceive;
    interface TokenReceiveMsg as UARTTokenReceive;
#endif	
    
#ifdef SENSORBOARD_ITS400
    interface StdControl as SHTMControl;
    interface StdControl as I2CControl;
    interface I2C;
    interface PMIC; 
    interface ADC as TempSensor;
    interface ADC as HumSensor; 
#endif
	
#ifdef SENSORBOARD_MTS400CB
    interface SplitControl as TempHumControl;
    interface ADC as Humidity;
    interface ADC as Temperature;
    interface ADCError as HumidityError;
    interface ADCError as TemperatureError;
    interface SplitControl as TaosControl;
    interface ADC as TaosCh0;
    interface ADC as TaosCh1;
#endif
#endif
  }
}

implementation {

#define BASESTATION_ADDR   0   // Address of the Base Station

#ifdef TINYOS2
#define LED0OFF      call Leds.led0Off()
#define LED0TOGGLE   call Leds.led0Toggle()
#define LED0ON       call Leds.led0On()
#define LED1OFF      call Leds.led1Off()
#define LED1TOGGLE   call Leds.led1Toggle()
#define LED1ON       call Leds.led1On()
#define LED2OFF      call Leds.led2Off()
#define LED2TOGGLE   call Leds.led2Toggle()
#define LED2ON       call Leds.led2On()
#else
#define LED0OFF      call Leds.redOff()
#define LED0TOGGLE   call Leds.redToggle()
#define LED0ON       call Leds.redOn()
#define LED1OFF      call Leds.greenOff()
#define LED1TOGGLE   call Leds.greenToggle()
#define LED1ON       call Leds.greenOn()
#define LED2OFF      call Leds.yellowOff()
#define LED2TOGGLE   call Leds.yellowToggle()
#define LED2ON       call Leds.yellowOn()
#endif

#ifdef TINYOS2
#define OS_OVERHEAD           (sizeof(message_header_t)+sizeof(message_footer_t))
#else
#define OS_OVERHEAD           7
#define TOS_NODE_ID           TOS_LOCAL_ADDRESS
#define AM_BROADCAST_ADDR     TOS_BCAST_ADDR 

typedef TOS_Msg  message_t;
typedef result_t error_t;
#endif

#ifdef TOSSIM
#define DEFAULT_TIMER1INTERVAL    1000                // Run slower in simulation because we have much more nodes
#else
#define DEFAULT_TIMER1INTERVAL    50
#endif

#define TIMER1_INTERVAL  (attr_desc[ATTR_MAINTIMER].value)   // base message transmission interval. Synchronized between nodes during peer discovery
#define TIMER2_INTERVAL  ((257*(timer_cycles+TOS_NODE_ID)+43) % TIMER1_INTERVAL)   // randomizer interval

#define MIN_RFPOWER      0
#define MAX_RFPOWER     31

typedef uint16_t ADDRESS;
typedef uint16_t ATTRIBUTE;

#ifdef USE_BLUSH
#define MAX_BLUSHFRAGMENTSIZE   80    // maximum number of bytes to send from mote to base station in one fragment
uint8_t blush_buffer[256];            // used to store incomming message fragments
int blushbuffer_size;
uint8_t blush_debugoutput = 0;        // if 1: send debug output to blush
#endif

message_t msg_outgoing, msguart_outgoing;
uint8_t uart_busy;

uint32_t timer_cycles;         // synchronized with timer of base station during peer discovery (if this node is not the base station)
uint32_t startup_cycles;       // cycles since startup of this node

#define RESET_CODE       0xa7
uint8_t reset_requested;   // if equals to RESET_CODE, perform reset
uint8_t reset_counter;

#define MAGIC_STARTUPSEQ "5687365804239"    // Magic startup sequence sent to the base station after startup/reboot

#include "lib/utils.h"
#include "lib/heapman.h"
#include "lib/uart.h"
#include "lib/config.h"
#include "lib/summary.h"
#include "lib/multicast.h"
#include "lib/sensor.h"
#include "lib/costmodel.h"

#include "lib/tree.h"
#include "lib/link.h"
#include "lib/page.h"
#include "lib/route.h"
#include "lib/forward.h"
#include "lib/querygroupopt.h"
#include "lib/queryplan.h"
#include "lib/querycomp.h"
#include "lib/windowjoin.h"

#include "lib/marshal.h"
#include "lib/logs.h"

#include "lib/heapman.c"
#include "lib/utils.c"
#include "lib/marshal.c"

#include "lib/summary.c"
#include "lib/multicast.c"
#include "lib/sensor.c"
#include "lib/uart.c"
#include "lib/costmodel.c"
#include "lib/logs.c"
#include "lib/config.c"
#include "lib/link.c"
#include "lib/tree.c"
#include "lib/route.c"
#include "lib/forward.c"
#include "lib/page.c"
#include "lib/querygroupopt.c"
#include "lib/queryplan.c"
#include "lib/querycomp.c"
#include "lib/windowjoin.c"

#define CHECK_FAIL (config.fail_time > 0 && timer_cycles > config.fail_time)

#ifndef TINYOS2
#ifdef SENSORBOARD_ITS400
//-------------------------------------------------------------------------
async event result_t TempSensor.dataReady(uint16_t data)
{
  atomic update_attr(ATTR_TEMP, -39.6+0.01*data, timer_cycles);
  return SUCCESS;
}

//-------------------------------------------------------------------------
async event result_t HumSensor.dataReady(uint16_t data)
{
  atomic update_attr(ATTR_HUM, (-28.0*data*data)/10000000+(405.0*data)/10000-4, timer_cycles);
  return SUCCESS;
}  

//-------------------------------------------------------------------------
#define TEMP1_SLAVE_ADDR 0x48
#define TEMP2_SLAVE_ADDR 0x4A
#define ADC_SLAVE_ADDR   0x34 //EUB unit, A0 low
#define LIGHT_SLAVE_ADDR 0x49

#define READ_OP 1
#define WRITE_OP 0

#define MAX_I2COPS 50
#define I2C_START 0
#define I2C_END   1
#define I2C_READ  2
#define I2C_WRITE 3
#define I2C_WAIT  4

#define ADC_REF 3 //reference voltage for ADC 

static uint8_t i2c_curslave = 0;
static uint8_t i2c_curtempslave = 0;
static bool i2c_firstlight = TRUE;
 
typedef struct I2COPSEQ_ENTRY 
{
  uint8_t op, param;
  uint32_t uwait_duration;
  uint8_t res;
} I2CIOSEQ_ENTRY;

I2CIOSEQ_ENTRY i2cop_seq[MAX_I2COPS];
uint8_t i2copseq_count, i2copseq_index, i2cresult_ofs; 

//-------------------------------------------------------------------------
uint32_t i2c_calclux(uint32_t vi_lo, uint32_t vi_hi, uint32_t ir_lo, uint32_t ir_hi) 
{
  uint32_t vi_i, ir_i;
  float vi, ir, ratio;

  vi_i = vi_lo + (vi_hi << 8);
  ir_i = ir_lo + (ir_hi << 8);
  
  if (vi_i == 0)
    return 0;
    
  vi = (float)vi_i;
  ir = (float)ir_i;
  ratio = ir/vi;	
          
  if (ratio <= 0.50) 
    return (0.03040*vi - 0.06200*vi*powf(ratio, 1.4));
  if (ratio <= 0.61) 
    return (0.02240*vi - 0.03100*ir);      
  if (ratio <= 0.80) 
    return (0.01280*vi - 0.01530*ir);       
  if (ratio <= 1.30) 
    return (0.00146*vi - 0.00112*ir);      
  return 0;
} 
	
//-------------------------------------------------------------------------
float i2c_calcvoltage(uint32_t msb, uint32_t lsb) 
{
  return ((((msb & 0x0f) << 8) | lsb)*ADC_REF/4.096);   // milivolts instead of volts (1000x higher)
} 

//-------------------------------------------------------------------------
void I2C_results() 
{
  switch (i2c_curslave)
  {
    case LIGHT_SLAVE_ADDR:   
      if (i2c_firstlight)
      {
        i2c_firstlight = 0;
        atomic update_attr(ATTR_LIGHT, SAMPLE_DEFAULTRESULT, timer_cycles);
      } else atomic update_attr(ATTR_LIGHT, i2c_calclux(i2cop_seq[i2cresult_ofs+6].res,
        i2cop_seq[i2cresult_ofs+13].res, i2cop_seq[i2cresult_ofs+20].res, i2cop_seq[i2cresult_ofs+27].res), timer_cycles);
      break;

    case ADC_SLAVE_ADDR:
      atomic update_attr(ATTR_ADC0+((i2cop_seq[6].res & 0x60) >> 5), i2c_calcvoltage(i2cop_seq[6].res, i2cop_seq[8].res),
        timer_cycles);    
      break;	    
      
    case TEMP1_SLAVE_ADDR:
    case TEMP2_SLAVE_ADDR:
      atomic update_attr(ATTR_TEMP, i2cop_seq[5].res, timer_cycles);
      break;  
  }
}

//-------------------------------------------------------------------------
task void I2C_nextcmd() 
{
  if (i2copseq_count == 0)
    return;
   
  if (i2copseq_index >= i2copseq_count) 
  {       
    I2C_results();
    i2copseq_count = 0;
    return;
  }
   
  switch (i2cop_seq[i2copseq_index].op) 
  {
    case I2C_START:  call I2C.sendStart(); break;
    case I2C_END:    call I2C.sendEnd(); break;
    case I2C_READ:   call I2C.read(i2cop_seq[i2copseq_index].param); break;
    case I2C_WRITE:  call I2C.write(i2cop_seq[i2copseq_index].param); break;
    case I2C_WAIT:   
      TOSH_uwait(i2cop_seq[i2copseq_index].uwait_duration);
      i2copseq_index++;
      post I2C_nextcmd();
      break;
  }
}
 
//-------------------------------------------------------------------------
event result_t I2C.sendStartDone() 
{
  i2copseq_index++;
  post I2C_nextcmd(); 
  return SUCCESS;
}

//-------------------------------------------------------------------------
event result_t I2C.sendEndDone() 
{
  i2copseq_index++;
  post I2C_nextcmd();
  return SUCCESS;
}

//-------------------------------------------------------------------------
event result_t I2C.readDone(char data) 
{
  i2cop_seq[i2copseq_index].res = data;
  i2copseq_index++;
  post I2C_nextcmd();
  return SUCCESS;
}

//-------------------------------------------------------------------------
event result_t I2C.writeDone(bool success) 
{
  i2cop_seq[i2copseq_index].res = success;
  i2copseq_index++;
  post I2C_nextcmd();
  return SUCCESS;
}

//-------------------------------------------------------------------------
void I2C_starttempsample()
{
  I2CIOSEQ_ENTRY temp_seq[] = {
    {I2C_START, 0, 0, 0},
    {I2C_WRITE, (i2c_curtempslave << 1) | WRITE_OP, 0, 0},
    {I2C_WRITE, 0, 0, 0},
    {I2C_START, 0, 0, 0},
    {I2C_WRITE, (i2c_curtempslave << 1) | READ_OP, 0, 0},
    {I2C_READ, TRUE, 0, 0},    
    {I2C_END, 0, 0, 0},
    {I2C_READ, FALSE, 0, 0}
  };
  
  i2c_curslave = i2c_curtempslave;
  memcpy(i2cop_seq, temp_seq, sizeof(temp_seq));
  i2copseq_count = sizeof(temp_seq)/sizeof(temp_seq[0]);  
}

//-------------------------------------------------------------------------
void I2C_startadcsample(uint8_t channel_index)
{
  I2CIOSEQ_ENTRY adc_seq[] = {
    {I2C_START, 0, 0, 0},
    {I2C_WRITE, (ADC_SLAVE_ADDR << 1) | WRITE_OP, 0, 0},
    {I2C_WRITE, 0x79 | (channel_index << 1), 0, 0},
    {I2C_WRITE, 0x82, 0, 0},
    {I2C_START, 0, 0, 0},
    {I2C_WRITE, (ADC_SLAVE_ADDR << 1) | READ_OP, 0, 0},
    {I2C_READ, TRUE, 0, 0},  
    {I2C_END, 0, 0, 0},  
    {I2C_READ, FALSE, 0, 0}
  };
  
  i2c_curslave = ADC_SLAVE_ADDR;
  memcpy(i2cop_seq, adc_seq, sizeof(adc_seq));
  i2copseq_count = sizeof(adc_seq)/sizeof(adc_seq[0]);  
}

//-------------------------------------------------------------------------
void I2C_startlightsample()
{
  I2CIOSEQ_ENTRY light_initseq[] = {
    {I2C_START, 0, 0, 0},     
    {I2C_WRITE, (LIGHT_SLAVE_ADDR << 1) | WRITE_OP, 0, 0},      
    {I2C_WRITE, 0xC0, 0, 0},	
    {I2C_WRITE, 0x03, 0, 0},
    {I2C_START, 0, 0, 0},   	 
    {I2C_WRITE, (LIGHT_SLAVE_ADDR << 1) | WRITE_OP, 0, 0},
    {I2C_WRITE, 0x81, 0, 0},   
    {I2C_WRITE, 0x12, 0, 0},    
    {I2C_WAIT, 0, 1000, 0}
  },
  
  light_seq[] = {
    {I2C_START, 0, 0, 0},    
    {I2C_WRITE, (LIGHT_SLAVE_ADDR << 1) | WRITE_OP, 0, 0},    	 
    {I2C_WRITE, 0x8C, 0, 0},     	    
    {I2C_START, 0, 0, 0}, 
    {I2C_WRITE, (LIGHT_SLAVE_ADDR << 1) | READ_OP, 0, 0},     	 
    {I2C_END, 0, 0, 0},     	
    {I2C_READ, FALSE, 0, 0},     	    
    {I2C_START, 0, 0, 0}, 
    {I2C_WRITE, (LIGHT_SLAVE_ADDR << 1) | WRITE_OP, 0, 0},
    {I2C_WRITE, 0x8D, 0, 0},    
    {I2C_START, 0, 0, 0},
    {I2C_WRITE, (LIGHT_SLAVE_ADDR << 1) | READ_OP, 0, 0},    
    {I2C_END, 0, 0, 0},
    {I2C_READ, FALSE, 0, 0},    
    {I2C_START, 0, 0, 0},
    {I2C_WRITE, (LIGHT_SLAVE_ADDR << 1) | WRITE_OP, 0, 0},
    {I2C_WRITE, 0x8E, 0, 0},
    {I2C_START, 0, 0, 0},
    {I2C_WRITE, (LIGHT_SLAVE_ADDR << 1) | READ_OP, 0, 0},
    {I2C_END, 0, 0, 0},
    {I2C_READ, FALSE, 0, 0},
    {I2C_START, 0, 0, 0},
    {I2C_WRITE, (LIGHT_SLAVE_ADDR << 1) | WRITE_OP, 0, 0},
    {I2C_WRITE, 0x8F, 0, 0},
    {I2C_START, 0, 0, 0},
    {I2C_WRITE, (LIGHT_SLAVE_ADDR << 1) | READ_OP, 0, 0},
    {I2C_END, 0, 0, 0},
    {I2C_READ, FALSE, 0, 0}
  };

  i2c_curslave = LIGHT_SLAVE_ADDR;
  i2cresult_ofs = 0;
  
  if (i2c_firstlight)
  {
    memcpy(i2cop_seq, light_initseq, sizeof(light_initseq));
    i2copseq_count = sizeof(light_initseq)/sizeof(light_initseq[0]);
    i2cresult_ofs = i2copseq_count;
  }
  
  memcpy(&i2cop_seq[i2copseq_count], light_seq, sizeof(light_seq));
  i2copseq_count += sizeof(light_seq)/sizeof(light_seq[0]);
}

//-------------------------------------------------------------------------
uint8_t I2C_startsample(uint8_t attr_index)
{
  if (i2copseq_count > 0)
    return 1;

  switch (attr_index)
  {
    case ATTR_ADC0:
    case ATTR_ADC1:
    case ATTR_ADC2:
    case ATTR_ADC3:  I2C_startadcsample(attr_index-ATTR_ADC0); break;
    case ATTR_LIGHT: I2C_startlightsample(); break;
    case ATTR_TEMP:  I2C_starttempsample(); break;
    default: return 1;
  }

  i2copseq_index = 0;
  post I2C_nextcmd();
  return 0;
}
#endif

#ifdef SENSORBOARD_MTS400CB
//-------------------------------------------------------------------------
task void stopTempHumControl() 
{
  call TempHumControl.stop();
  return;
}
  
//-------------------------------------------------------------------------  
task void stopTaosControl()
{
  call TaosControl.stop();
  return;
}

//-------------------------------------------------------------------------
uint8_t MTS400CB_istemp;

async event result_t Temperature.dataReady(uint16_t data) 
{
  atomic update_attr(ATTR_TEMP, data, timer_cycles);
  post stopTempHumControl();
  return SUCCESS;
}

//-------------------------------------------------------------------------  
async event result_t Humidity.dataReady(uint16_t data) 
{
  atomic update_attr(ATTR_HUM, data, timer_cycles);
  post stopTempHumControl();
  return SUCCESS;
}
  
//-------------------------------------------------------------------------  
event result_t TempHumControl.startDone() 
{
  if (MTS400CB_istemp)
    call Temperature.getData();
  else call Humidity.getData();
  return SUCCESS;
}
  
//-------------------------------------------------------------------------  
event result_t TempHumControl.initDone() 
{
  return SUCCESS;
}
  
//-------------------------------------------------------------------------  
event result_t TempHumControl.stopDone() 
{
  return SUCCESS;
}

//-------------------------------------------------------------------------  
event result_t HumidityError.error(uint8_t token) 
{
  atomic update_attr(ATTR_HUM, SAMPLE_DEFAULTRESULT, timer_cycles);
  call TempHumControl.stop();
  return SUCCESS;
}
    
//-------------------------------------------------------------------------	
event result_t TemperatureError.error(uint8_t token) 
{
  atomic update_attr(ATTR_TEMP, SAMPLE_DEFAULTRESULT, timer_cycles);
  call TempHumControl.stop();
  return SUCCESS;
}

//-------------------------------------------------------------------------
async event result_t TaosCh1.dataReady(uint16_t data) 
{                            // ignore data from channel 1
  post stopTaosControl();
  return SUCCESS;
}
  
//-------------------------------------------------------------------------  
async event result_t TaosCh0.dataReady(uint16_t data) 
{
  atomic update_attr(ATTR_LIGHT, data & 0xff, timer_cycles);
  post stopTaosControl();
  return SUCCESS;
}

//-------------------------------------------------------------------------  
event result_t TaosControl.startDone()
{
  return call TaosCh0.getData();
}
  
//-------------------------------------------------------------------------  
event result_t TaosControl.initDone() 
{
  return SUCCESS;
}
  
//-------------------------------------------------------------------------  
event result_t TaosControl.stopDone() 
{
  return SUCCESS;
}
#endif
#endif

//-------------------------------------------------------------------------
void handle_rfidmsg(ADDRESS rfid, int8_t msg_power)
{
  DBGOUT("Received RFID %d with power %hhd", rfid, msg_power);  
  atomic {
    attrqueue_push(ATTR_RFID, rfid, 1);
    attrqueue_push(ATTR_RFIDSIG, (int)msg_power+128, 0);
  }
}

//-------------------------------------------------------------------------
void hardware_sampling()
{
  int i;
#ifdef TOSSIM 
  ATTRIBUTE v;
#endif
#ifdef SENSORBOARD_ITS400
  uint8_t voltage; 
#endif

  for (i = 0; i < MAX_ATTRDESC; i++)
    if (attr_desc[i].is_hardware && attr_desc[i].needs_sampling && attr_desc[i].is_sampling)
    {
      DBGOUT("Attribute %d being sampled", i);
      if (timer_cycles-attr_desc[i].sample_begin > SAMPLE_TIMEOUT)
      {
        DBGOUT("Timeout");
        atomic update_attr(i, SAMPLE_DEFAULTRESULT, timer_cycles);
      }
      return;
    }  
  
  for (i = 0; i < MAX_ATTRDESC; i++)
    if (attr_desc[i].is_hardware && attr_desc[i].needs_sampling && attr_desc[i].is_sampling == 0)
    {		
#ifdef TOSSIM
      DBGOUT("Sampling attribute %s", attr_name[i]);
      if (attr_desc[i].is_dynamic && sensor_offlinesample(attr_desc[i].sample_number+1, &v,
        query_buffer[0].spec.sel_est[REL_J]) == 0)        // sample_count+1 for compatability with old results
        update_attr(i, v, timer_cycles);
      continue;
#else
      DBGOUT("Sampling attribute %d", i);
      attr_desc[i].is_sampling = 1;
      attr_desc[i].sample_begin = timer_cycles;
      switch (i)
      {		  
#ifdef SENSORBOARD_ITS400	  
//        case ATTR_TEMP:  call TempSensor.getData(); break;
        case ATTR_HUM:   call HumSensor.getData(); break;
        case ATTR_ADC0:
        case ATTR_ADC1:
        case ATTR_ADC2:
        case ATTR_ADC3:
        case ATTR_LIGHT:
        case ATTR_TEMP:
          if (I2C_startsample(i))          
          {
            DBGOUT("I2C busy, waiting");
            attr_desc[i].is_sampling = 0;   // wait if I2C is busy sampling another attribute
          }
          break;

        case ATTR_BATT:  
          call PMIC.getBatteryVoltage(&voltage);
          atomic update_attr(ATTR_BATT, 10.35*voltage+2650, timer_cycles); // battery voltage is in milivolts
          break;
#endif
 
#ifdef SENSORBOARD_MTS400CB
        case ATTR_TEMP:   MTS400CB_istemp = 1; call TempHumControl.start(); break;		  
        case ATTR_HUM:    MTS400CB_istemp = 0; call TempHumControl.start(); break; 		  
        case ATTR_LIGHT:  call TaosControl.start(); break;		
#endif

        default:          
		  atomic update_attr(i, SAMPLE_DEFAULTRESULT, timer_cycles);   // hardware sensor not bound, will never get sampled
          break;
      }
	  
      return;    // dont sample from more than 1 sensor per cycle	  
#endif
    }
}

//-------------------------------------------------------------------------
#ifdef TINYOS2
event void SendUART.sendDone(message_t* bufPtr, error_t error)
{
  uart_busy = 0;
}

//-------------------------------------------------------------------------
event message_t *ReceiveUART.receive(message_t *bufPtr, void *payload, uint8_t len)
{
  atomic enqueue_uartmsg(0, len, payload, TOS_NODE_ID);
  return bufPtr;
}
#else
#ifdef USE_BLUSH
//-------------------------------------------------------------------------
void send_blushmsg(uint8_t len, uint8_t *data)
{
  char *p, s[2*(MAX_BLUSHFRAGMENTSIZE+2)];     // is_lastfragment + MAX_BLUSHFRAGMENTSIZE + '\r' + \0
  uint8_t i, j, count, fragment_count = DIV_CEIL(len, MAX_BLUSHFRAGMENTSIZE);
  
  DBGOUT("Sending Blush message to server (%d, %d)", len, fragment_count);
  for (i = 0; i < fragment_count; i++)
  {   
    sprintf(s, "%02x", (i == fragment_count-1));
    p = s+2;
        
    count = MIN(len, MAX_BLUSHFRAGMENTSIZE);
    for (j = 0; j < count; j++)
    {     
      sprintf(p, "%02x", data[j]);
      p += 2;
    }
    sprintf(p, "\r");
    
    trace(DBG_USR1, "\nFragment: %s\n", s);
    
    len -= count;
    data += count;
  }
}

//-------------------------------------------------------------------------
command BluSH_result_t BlushCommand.getName(char *buff, uint8_t len) 
{
  strcpy(buff, "Command"); // get parameters of command and execute
  return BLUSH_SUCCESS_DONE;
}

//------------------------------------------------------------------------- 
command BluSH_result_t BlushCommand.callApp(char *cmdBuff, uint8_t cmdLen, char *resBuff, uint8_t resLen) 
{
  uint8_t last_fragment = 0;
  char *p = strstr(cmdBuff, " ");
  int t, count = 0;
  
  if (p == NULL)
    trace(DBG_USR1, "Error: Invalid command\n");
  else {
    uart_busy = 1;
    
    p++;
    while (isalnum(*p))
    {
      count++;     
      sscanf(p, "%02x", &t);
    
      if (count == 1)
        last_fragment = t;
      else blush_buffer[blushbuffer_size++] = t;
    
      if (blushbuffer_size >= sizeof(blush_buffer))
      {
        trace(DBG_USR1, "Error: Incoming blush message too large\n");
        blushbuffer_size = 0;
        uart_busy = 0;
        last_fragment = 0;
        break;
      }
    
      p += 2;
    }
    
    if (last_fragment)
    {
      trace(DBG_USR1, "Note: Transfer complete (%d bytes)\n", blushbuffer_size);
      atomic enqueue_uartmsg(0, blushbuffer_size, blush_buffer, TOS_NODE_ID);
      blushbuffer_size = 0;
      uart_busy = 0;
    }
  }

  return BLUSH_SUCCESS_DONE; 
}  

//------------------------------------------------------------------------- 
command BluSH_result_t BlushAttr.getName(char *buff, uint8_t len) 
{
  strcpy(buff, "Attr"); // get parameters of command and execute
  return BLUSH_SUCCESS_DONE;
}

//------------------------------------------------------------------------- 
command BluSH_result_t BlushAttr.callApp(char *cmdBuff, uint8_t cmdLen, char *resBuff, uint8_t resLen) 
{  
  uint8_t attr_index;
  ATTRIBUTE value;
  
  if (cmdLen > strlen("Attr"))  
  {
    if (strstr(cmdBuff, "Attr Get"))
    {    
      sscanf(cmdBuff, "Attr Get %hhu %hu", &attr_index, &value);
      if (attr_index < MAX_ATTRDESC)
      {
        trace(DBG_USR1, "Current value for attribute %d: %d\n", attr_index, attr_desc[attr_index].value);  
        if (attr_desc[attr_index].is_hardware && value && peer_discovery)     // only allow attribute updates during peer discovery
        {
          trace(DBG_USR1, "Attribute %d scheduled for sampling\n", attr_index);
          sample_attr(attr_index);
        }    
      } else trace(DBG_USR1, "Error: Invalid attribute (%d)\n", attr_index);  
    } else if (strstr(cmdBuff, "Attr Set"))
    {
      sscanf(cmdBuff, "Attr Set %hhu %hu", &attr_index, &value);
      if (attr_index < MAX_ATTRDESC && peer_discovery)                        // only allow attribute updates during peer discovery
      {
        trace(DBG_USR1, "Updating attribute %d with value %d\n", attr_index, value);
        update_attr(attr_index, value, timer_cycles);
      } else trace(DBG_USR1, "Error: Invalid attribute (%d)\n", attr_index);  
    } 
  } else trace(DBG_USR1, "Error: Invalid Attr request\n");
  
  return BLUSH_SUCCESS_DONE; 
} 

//------------------------------------------------------------------------- 
command BluSH_result_t BlushDebug.getName(char *buff, uint8_t len) 
{
  strcpy(buff, "Debug"); // get parameters of command and execute
  return BLUSH_SUCCESS_DONE;
}

//------------------------------------------------------------------------- 
command BluSH_result_t BlushDebug.callApp(char *cmdBuff, uint8_t cmdLen, char *resBuff, uint8_t resLen) 
{  
  uint8_t k;

  sscanf(cmdBuff, "Debug %hhu", &k);
  switch (k)
  {
    case 0:
#ifdef SAVE_DEBUG    
      call Timer3.stop();
#endif      
    case 1:  
      blush_debugoutput = k; 
      trace(DBG_USR1, "Debug: %d\n", blush_debugoutput);
      break;
    
    case 2: 
#ifdef SAVE_DEBUG
    call Timer3.start(TIMER_REPEAT, 500);
#else
    trace(DBG_USR1, "Error: SAVE_DEBUG not defined\n");
#endif 
    break;
    
    default: 
      trace(DBG_USR1, "Error: Unknown debug option: %d\n", k);
  }
  
  return BLUSH_SUCCESS_DONE; 
} 
#else
event result_t UARTSend.sendDone(message_t* bufPtr, result_t success) 
{
  uart_busy = 0;
  return SUCCESS;
}

//-------------------------------------------------------------------------
event message_t *UARTReceive.receive(message_t *msg) 
{
  atomic enqueue_uartmsg(0, msg->length, msg->data, TOS_NODE_ID);
  return msg;
}

//-------------------------------------------------------------------------
event TOS_MsgPtr UARTTokenReceive.receive(message_t *msg, uint8_t token) 
{
  call UARTTokenReceive.ReflectToken(token); 
  atomic enqueue_uartmsg(0, msg->length, msg->data, TOS_NODE_ID); 
  return msg;
}
#endif
#endif

//-------------------------------------------------------------------------
#ifdef TINYOS2
message_t *do_receive(message_t *bufPtr, void *payload, uint8_t len)
{
  int8_t msg_power = 0;
INIT_STACKTEST

#ifndef TOSSIM
  msg_power = call CC2420Packet.getRssi(bufPtr);
#endif

  atomic {
#ifdef TOSSIM
    if (CHECK_FAIL == 0)
#endif    
      decode_message(call AMPacket.source(bufPtr), call AMPacket.destination(bufPtr), payload, len, msg_power);
  }

END_STACKTEST

  return bufPtr;
}
#else
message_t *do_receive(message_t *bufPtr)
{
#ifdef EVENLENGTH_MSG
  if (bufPtr->data[0] & 0x80)
  {
    bufPtr->data[0] &= 0x7f;
    bufPtr->length--;
  }
#endif

  atomic {
#ifdef TOSSIM  
    if (CHECK_FAIL == 0)
#endif
      if (bufPtr->length >= DIV_CEIL(ADDRDOMAIN_BITS, 8))
        decode_message(bufPtr->data[0], bufPtr->addr, &bufPtr->data[DIV_CEIL(ADDRDOMAIN_BITS, 8)], 
	      bufPtr->length-DIV_CEIL(ADDRDOMAIN_BITS, 8), bufPtr->strength);        
  }

  return bufPtr;
}
#endif

//-------------------------------------------------------------------------
#ifdef TINYOS2
event message_t *ReceiveT.receive(message_t *bufPtr, void *payload, uint8_t len)
{
  return do_receive(bufPtr, payload, len);
}
#else
event message_t *ReceiveMsg.receive(message_t *bufPtr)
{     
  if (!bufPtr->crc)
    return bufPtr;
    
  if (bufPtr->group == TOS_AM_GROUP)
    return do_receive(bufPtr);    
  else {
    handle_rfidmsg(bufPtr->group-TOS_AM_GROUP, bufPtr->strength);
    return bufPtr;
  }
}
#endif

//-------------------------------------------------------------------------
#ifdef TINYOS2
event message_t *ReceiveS.receive(message_t *bufPtr, void *payload, uint8_t len)
{
  return do_receive(bufPtr, payload, len);
}
#endif

//-------------------------------------------------------------------------
#ifdef TINYOS2
event void Timer2.fired()
#else
event result_t Timer2.fired()
#endif
{
  ADDRESS addr;
  uint8_t len, send_power;

INIT_STACKTEST

  atomic {
#ifdef TOSSIM
    if (CHECK_FAIL == 0)
    {
#endif
      memset(&msguart_outgoing, 0, sizeof(msguart_outgoing)); 
      if (TOS_NODE_ID == BASESTATION_ADDR && uart_busy == 0 &&
        update_uartbuffer((uint8_t *) msguart_outgoing.data, &len) == 0)
      {
#ifndef TOSSIM
#ifdef USE_BLUSH
        send_blushmsg(len, msguart_outgoing.data);
#else
#ifdef TINYOS2
        call AMPacket.setSource(&msguart_outgoing, TOS_NODE_ID);
        call SendUART.send(addr, &msguart_outgoing, len);
#else
        msguart_outgoing.length = len;
        call UARTSend.send(&msguart_outgoing);
#endif
#endif
#endif
      }

      update_payloadbuffer();
      update_mergebuffer();      

#ifdef TINYOS2
      call Packet.clear(&msg_outgoing);  
      if (update_buffer(&addr, (uint8_t *) msg_outgoing.data, &len, &send_power) == 0)
      {
        call AMPacket.setSource(&msg_outgoing, TOS_NODE_ID);
#ifdef DISABLE_HARDWAREACK
        call PacketAck.noAck(&msg_outgoing);
#else
        call PacketAck.requestAck(&msg_outgoing);
#endif
        update_result(call AMSend.send(addr, &msg_outgoing, len));
      }
#else
      memset(&msg_outgoing, 0, sizeof(msg_outgoing));
      if (update_buffer(&addr, (uint8_t *) &msg_outgoing.data[DIV_CEIL(ADDRDOMAIN_BITS, 8)], &len, &send_power) == 0)
      {
        msg_outgoing.group = TOS_AM_GROUP;
        msg_outgoing.addr = addr;
        msg_outgoing.data[0] = TOS_NODE_ID;
        len += DIV_CEIL(ADDRDOMAIN_BITS, 8); 
        msg_outgoing.length = len;                
#ifdef EVENLENGTH_MSG        
        if (msg_outgoing.length & 0x01)
        {
          msg_outgoing.length++;
          msg_outgoing.data[0] |= 0x80;
        }
#endif        
        call CC2420Control.SetRFPower(send_power);
        update_result(call BareSendMsg.send(&msg_outgoing));
      }      
      
#endif
	  
#ifdef TOSSIM
    }
#endif    
  }

END_STACKTEST

#ifndef TINYOS2
  return SUCCESS;
#endif
}

//-------------------------------------------------------------------------
#ifdef TINYOS2
event void AMSend.sendDone(message_t *bufPtr, error_t error)
{
INIT_STACKTEST

  atomic {
#ifdef TOSSIM
    if (CHECK_FAIL == 0)
#endif    
      if (&msg_outgoing == bufPtr)
        handle_senddone(call PacketAck.wasAcked(&msg_outgoing), error);
  }
  
END_STACKTEST
}
#else
event result_t BareSendMsg.sendDone(message_t *bufPtr, error_t error)
{
INIT_STACKTEST

  atomic {
#ifdef TOSSIM
    if (CHECK_FAIL == 0)
#endif    
      if (&msg_outgoing == bufPtr)
        handle_senddone(0, error);
  }

END_STACKTEST
  return SUCCESS;
}
#endif

//-------------------------------------------------------------------------
void display_globalstatus()
{
  switch (global_status & 0x03)
  {
    case 0:  LED0OFF; break;
    case 1:  if (timer_cycles % 2 == 0) LED0TOGGLE; break;
    case 2:  LED0TOGGLE; break;
    case 3:  LED0ON; break;
  }

  switch ((global_status >> 2) & 0x03)
  {
    case 0:  LED1OFF; break;
    case 1:  if (timer_cycles % 2 == 0) LED1TOGGLE; break;
    case 2:  LED1TOGGLE; break;
    case 3:  LED1ON; break;
  }
  
  if (outputbuffer_size > 0)
    LED2TOGGLE; 
  else LED2OFF;
}

//-------------------------------------------------------------------------
#ifdef TOSSIM
void timer_storestats()
{
  store_state();
  log_routestats();
  if (config.start_route)
    store_joinstate();
}
#endif

//-------------------------------------------------------------------------
#ifdef TINYOS2
event void Timer1.fired()
#else
event result_t Timer1.fired()
#endif
{
#ifdef TOSSIM
  uint8_t busy_flag;
#endif

INIT_STACKTEST

#ifndef TOSSIM
  if (reset_requested == RESET_CODE)
  {
    set_globalstatus(GS_RESETREQUESTED);
    if (node_idle())
    {
      reset_counter++;
      if (reset_counter > 10)
      {
        DBGOUT("Resetting...");
        call Reset.reset();
      }
    } else reset_counter = 0;
  }
#endif  

  if (attr_desc[ATTR_MAINTIMER].sampling_time > 0)        // if the main timer interval has recently been update, restart the timer with the new interval
  {
    attr_desc[ATTR_MAINTIMER].sampling_time = 0;
    call Timer1.stop();
#ifdef TINYOS2    
    call Timer1.startPeriodic(TIMER1_INTERVAL);
#else    
    call Timer1.start(TIMER_REPEAT, TIMER1_INTERVAL);
#endif    
  } 

#ifdef TOSSIM
  if (CHECK_FAIL)
  {
    timer_storestats();            // store traffic info for failed nodes, needed for analysis
END_STACKTEST
    return;
  }

  if (TOS_NODE_ID == BASESTATION_ADDR)
    DBGOUT("Cycle: %d", timer_cycles);

  busy_flag = (timer_cycles <= 10 || outputbuffer_sending || outputbuffer_size > 0 ||
    mergebuffer_size > 0 || ((config.start_route == 0 || config.start_join) && check_queriesrunning()));
  if ((config.load_trees || tree_phase == TREE_COUNT) && check_routeterminate(busy_flag, config.node_count))
  {
    DBGOUT("Shutting down...");
    exit(0);
  }

  if (busy_flag == 0 && (timer_cycles % 10) == 0)
    timer_storestats();
#endif

  timer_cycles++;
  startup_cycles++;

#ifndef TOSSIM  
  display_globalstatus();
#endif  

#ifdef TINYOS2  
  call Timer2.startOneShot(TIMER2_INTERVAL);
#else
  call Timer2.start(TIMER_ONE_SHOT, TIMER2_INTERVAL);
#endif
  
  atomic {
    hardware_sampling(); 
    if (tree_phase < TREE_COUNT)
      timer_treeconstruction();        
    else {
      timer_querycomp();     
      if ((timer_cycles & 0x3ff) == (TOS_NODE_ID & 0x3ff))   // every 1024 cycles...
      {
        send_discoverymsg(AM_BROADCAST_ADDR, MSG_NOCONFIRM, 1, 1);   // synchronize clocks
        if (config.merge_paths & 0x01)                               // and exchange forward buffers
          timer_forwardbufferexchange();
      }    
    }
  }

END_STACKTEST

#ifndef TINYOS2
  return SUCCESS;
#endif
}

//-------------------------------------------------------------------------
#ifndef TINYOS2
#ifdef USE_BLUSH
#ifdef SAVE_DEBUG
event result_t Timer3.fired()
{
  atomic {
    if (save_debug[savedebug_begin])
    {
      trace(DBG_USR1, "%s\n", save_debug[savedebug_begin]);
      free(save_debug[savedebug_begin]);
      save_debug[savedebug_begin] = NULL;
      savedebug_begin = (savedebug_begin+1) % MAX_DEBUGSTR;
    }
  }  

  return SUCCESS;
}
#endif
#endif
#endif

//-------------------------------------------------------------------------
#ifdef TINYOS2
event void RadioControl.startDone(error_t err)
{
  call Timer1.startPeriodic(TIMER1_INTERVAL);
}
#else
command result_t StdControl.start()
{
  call Timer1.start(TIMER_REPEAT, TIMER1_INTERVAL);
  call CommControl.start();
  
  call CC2420Control.disableAutoAck();
  call CC2420Control.SetRFPower(MAX_RFPOWER);
  
#ifndef USE_BLUSH  
  call UARTControl.start();
#endif  
  
#ifdef SENSORBOARD_ITS400  
  call PMIC.enableSBVoltage_High(TRUE, LDO_TRIM_3P0);
  call I2CControl.start();
  call SHTMControl.start();
#endif

#ifdef SENSORBOARD_MTS400CB
  call HumidityError.enable();                
  call TemperatureError.enable();              
#endif
	 
  return SUCCESS;
}
#endif

//-------------------------------------------------------------------------
#ifdef TINYOS2
event void RadioControl.stopDone(error_t err)
#else
command result_t StdControl.stop()
#endif
{
  call Timer1.stop();
  call Timer2.stop();
  
#ifndef TINYOS2
#ifndef USE_BLUSH
  call UARTControl.stop();
#endif
  
#ifdef SENSORBOARD_ITS400
  call PMIC.enableSBVoltage_High(FALSE, LDO_TRIM_3P0);
  call I2CControl.stop(); 
  call SHTMControl.stop();
#endif
  
  return SUCCESS;
#endif  
}

//-------------------------------------------------------------------------
#ifdef TINYOS2
event void UARTControl.startDone(error_t err)
{
}

//-------------------------------------------------------------------------
event void UARTControl.stopDone(error_t err)
{
}
#endif

//-------------------------------------------------------------------------
#ifdef TINYOS2
event void Boot.booted()
#else
command result_t StdControl.init()
#endif
{
#ifndef TINYOS2
  addr_marker = 0;  // must reference this TOS_LOCAL_ADDR marker variable for it to be compiled into binary 
#endif

  timer_cycles = 0;
  startup_cycles = 0;
  
  reset_requested = 0; 
  reset_counter = 0;
  
  uart_busy = 0;
  srand(TOS_NODE_ID); 
  
  init_mconfig();
  init_mheapman();
  init_msensor();        // must initialize before config and logs, not to overwrite summary and attribute data
  init_muart();
#ifdef TOSSIM
  read_config();
#endif
  init_mutils();
  init_mmarshal();
  init_mlogs();
  init_mlink();
  init_mtree();
  init_mroute();
  init_mforward();
  init_mwindowjoin();   // must initialize before queryplan, so that it does not overwrite loaded join state
  init_mqueryplan();
  init_mquerycomp();

#ifdef TINYOS2
  call RadioControl.start();
  call UARTControl.start();
#else
  call Leds.init();
  call CommControl.init();

#ifdef USE_BLUSH  
  blushbuffer_size = 0;
#else  
  call UARTControl.init();
#endif  
  
#ifdef SENSORBOARD_ITS400  
  i2copseq_count = 0;
  i2copseq_index = 0;
  i2c_curtempslave = TEMP2_SLAVE_ADDR;   
  call I2CControl.init();
  call SHTMControl.init();
#endif 
  
#ifdef SENSORBOARD_MTS400CB
  MTS400CB_istemp = 0;
  call TaosControl.init();
  call TempHumControl.init();    
#endif
  
  if (TOS_NODE_ID == BASESTATION_ADDR)
    trace(DBG_USR1, "Startup %s...\n", MAGIC_STARTUPSEQ);    // notify base station server that we have started up. Output magic sequence
 
  return SUCCESS;
#endif  
}

}
